/*
port1=bottom left drive
port2=top left drive
port3=bottom right drive
port4=top right drive
Top left drive motor is reversed;
Bottom right drive motor is reversed;
*/
#ifndef VEX_FUNCTIONS_H
#include "Vex_Functions.h"
#endif

#define VEX_FUNCTIONS_C

void drive(int leftDesired, int rightDesired);


// Autonomous constants
#define TARGET_KI 0
#define TARGET_KP 3
#define  ENCODERACCEPTANCERANGE 15
#define SPEEDACCEPTANCERANGE 5
 int leftGeartoothPosition = 0;
 int rightGeartoothPosition = 0;
 //int leftEncoder = 0;
 //int rightEncoder = 0;
 long accumulatedError[10]= {0,0,0,0,0,0,0,0,0,0};



 bool withinRange(int a, int target, int distance)
{
  if (a-target < distance && a-target > -distance){
    return true;
  }
  else{
    return false;
  }
}



// Tells you what motor power to use in order to get to the target accurately.
int getPID(int target,int current,int accumulatedslot, int kp, int ki)
{

  int outI = 0;
  int outP = 0;
  //==========================================P==============================================================
  int error  = target - current;

  //Error is target distance (target) - what we are at now :: Kp is the slope of the multiplier
  // If the distance from the target is large/small how much force it putout
  if (error > 127/kp)
  {
       outP = 127;
  }
  else if (error < -127/kp)
  {
       outP = -127;
  }
  else
  {
      outP = kp*error;
  }

  //=============================================I===========================================================
  accumulatedError[accumulatedslot] = accumulatedError[accumulatedslot] + error;
  if (ki !=0){
	  //If we haven't been on the target for awhile because P is too low. this will force the motors to go faster.
	  if (accumulatedError[accumulatedslot]/100 > 127/ki)
	  {
	      outI = 127;
	  }
	  else if (accumulatedError[accumulatedslot]/100 < -127/ki)
	  {
	      outI = -127;
	  }
	  else
	  {
	       outI = ki*(accumulatedError[accumulatedslot]/100);
	  }
  }
  else{
    outI = 0;
  }
  //===========================================OUT PUT/RETURN================================================
  // If the output of I AND P are greater or less then 127 then it makes it +-127.
  return limitMotorPower(outP+outI); //outI+outP);
}

//limitMotorPower function is tested and working.
// If the output of I AND P are greater or less then 127 then it makes it +-127.
int limitMotorPower (int number)
{
  if (number > 127)
  {
       return 127;
  }
  else if (number < -127)
  {
       return -127;
  }
  else
  {
      return number;
  }
}





//================================ FUNCTION TO DRIVE THE ROBOT STRAIGHT====================================

bool driveStraight (int target)
{
  //drives the each side 2,1 (left). 3,4 (right). Using the PID which calulates and returns a number.
  int left =limitMotorPower (getPID(target,SensorValue[leftEncoder],1,1,0));//+ getPID(SensorValue[rightEncoder],SensorValue[leftEncoder],3,0,0));
  int right=limitMotorPower (getPID(target,SensorValue[rightEncoder],2,1,0));//+ getPID(SensorValue[leftEncoder],SensorValue[rightEncoder],4,0,0));
  drive(left, right);
/*TODO: If the bot is going very fast by its target there will be a MOMENT when
  the error ==0 but that doesn't mean it is time to switch to the next stage. You want to make
  sure it is at/near its target AND that it is stable/resting. Also we have a left and right side that both
  need to be in the right spot before we return the "all good" signal*/


  //==================================== Return True  if the bot is at its target===========================


 bool ret = 0;



  //TODO: replace "==target" with withinrange(_____,target)
  if (withinRange (SensorValue[rightEncoder], target, ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder], target, ENCODERACCEPTANCERANGE) && SensorValue[rightEncoder] - rightGeartoothPosition < SPEEDACCEPTANCERANGE && SensorValue[leftEncoder] - leftGeartoothPosition < SPEEDACCEPTANCERANGE)
  {
   ret = true;
   //TODO: reset encoders to 0
   SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
  }
  leftGeartoothPosition = SensorValue[leftEncoder];
  rightGeartoothPosition = SensorValue[rightEncoder];

  return ret;

}


//=====================================================
//turn is measured in degrees of turn from the center of the bot & 0 being straight forward, positive numbers turn it to the left.
bool driveTurn (int degree)
{
  //Calculations assume Bot radius=8", 1 wheel rotation = 12.56", 1 encoder rotation=90 clicks
  int target = .332 * (degree * 4);
  int temp2;
  motor[port2]= motor[port1]= motor[port9]=limitMotorPower (getPID(-target,SensorValue[leftEncoder],1,TARGET_KP,TARGET_KI));//+ 0*getPID(-SensorValue[rightEncoder],SensorValue[leftEncoder],3,0,1));
  motor[port3]= motor[port4]= motor[port10]= temp2 = limitMotorPower (getPID(target,SensorValue[rightEncoder],2,TARGET_KP,TARGET_KI));//+ 0*getPID(-SensorValue[leftEncoder],SensorValue[rightEncoder],4,0,1));

  bool ret = 0;
  if (withinRange(SensorValue[rightEncoder] ,target,ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder] , -target,ENCODERACCEPTANCERANGE) && SensorValue[rightEncoder] - rightGeartoothPosition < ENCODERACCEPTANCERANGE && SensorValue[leftEncoder] - leftGeartoothPosition < ENCODERACCEPTANCERANGE)
  {
    ret = true;

    SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
  }
  leftGeartoothPosition = SensorValue[leftEncoder];
  rightGeartoothPosition = SensorValue[rightEncoder];
  return ret;
}





/*void testAuto()
{
 while (driveStraight(100)==false)
 {

 }

 while(vexRT[btn8R]==0)
 {


 }

 while (driveStraight(-100)==false)
  {

  }

 while(vexRT[btn8R]==1)
 {


 }

  while (driveTurn(90)==false)
  {

  }

 while(vexRT[btn8R]==0)
 {


 }

 while (driveTurn (180)==false)
 {

 }

 testAuto();

}*/


/*
port1=bottom left drive
port2=top left drive
port3=bottom right drive
port4=top right drive
Top left drive motor is reversed;
Bottom right drive motor is reversed;
*/
/*
#ifndef VEX_FUNCTIONS_H
#include "Vex_Functions.h"
#endif

#define VEX_FUNCTIONS_C

void drive(int leftDesired, int rightDesired);


// Autonomous constants
#define TARGET_KI 0
#define TARGET_KP 3
#define  ENCODERACCEPTANCERANGE 15
#define SPEEDACCEPTANCERANGE 5
 int leftGeartoothPosition = 0;
 int rightGeartoothPosition = 0;
 //int leftEncoder = 0;
 //int rightEncoder = 0;
 long accumulatedError[10]= {0,0,0,0,0,0,0,0,0,0};



 bool withinRange(int a, int target, int distance)
{
  if (a-target < distance && a-target > -distance){
    return true;
  }
  else{
    return false;
  }
}



// Tells you what motor power to use in order to get to the target accurately.
int getPID(int target,int current,int accumulatedslot, int kp, int ki)
{

  int outI = 0;
  int outP = 0;
  //==========================================P==============================================================
  int error  = target - current;

  //Error is target distance (target) - what we are at now :: Kp is the slope of the multiplier
  // If the distance from the target is large/small how much force it putout
  if (error > 127/kp)
  {
       outP = 127;
  }
  else if (error < -127/kp)
  {
       outP = -127;
  }
  else
  {
      outP = kp*error;
  }

  //=============================================I===========================================================
  accumulatedError[accumulatedslot] = accumulatedError[accumulatedslot] + error;
  if (ki !=0){
	  //If we haven't been on the target for awhile because P is too low. this will force the motors to go faster.
	  if (accumulatedError[accumulatedslot]/100 > 127/ki)
	  {
	      outI = 127;
	  }
	  else if (accumulatedError[accumulatedslot]/100 < -127/ki)
	  {
	      outI = -127;
	  }
	  else
	  {
	       outI = ki*(accumulatedError[accumulatedslot]/100);
	  }
  }
  else{
    outI = 0;
  }
  //===========================================OUT PUT/RETURN================================================
  // If the output of I AND P are greater or less then 127 then it makes it +-127.
  return limitMotorPower(outP+outI); //outI+outP);
}

//limitMotorPower function is tested and working.
// If the output of I AND P are greater or less then 127 then it makes it +-127.
int limitMotorPower (int number)
{
  if (number > 127)
  {
       return 127;
  }
  else if (number < -127)
  {
       return -127;
  }
  else
  {
      return number;
  }
}





//================================ FUNCTION TO DRIVE THE ROBOT STRAIGHT====================================

bool driveStraight (int target)
{
  //drives the each side 2,1 (left). 3,4 (right). Using the PID which calulates and returns a number.
  int left =limitMotorPower (getPID(target,SensorValue[leftEncoder],1,1,0));//+ getPID(SensorValue[rightEncoder],SensorValue[leftEncoder],3,1,0));
  int right=limitMotorPower (getPID(target,SensorValue[rightEncoder],2,1,0));//+ getPID(SensorValue[leftEncoder],SensorValue[rightEncoder],4,1,0));
  drive(left, right);
/*TODO: If the bot is going very fast by its target there will be a MOMENT when
  the error ==0 but that doesn't mean it is time to switch to the next stage. You want to make
  sure it is at/near its target AND that it is stable/resting. Also we have a left and right side that both
  need to be in the right spot before we return the "all good" signal*/


  //==================================== Return True  if the bot is at its target===========================

/*
 bool ret = 0;



  //TODO: replace "==target" with withinrange(_____,target)
  if (withinRange (SensorValue[rightEncoder], target, ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder], target, ENCODERACCEPTANCERANGE) && SensorValue[rightEncoder] - rightGeartoothPosition < SPEEDACCEPTANCERANGE && SensorValue[leftEncoder] - leftGeartoothPosition < SPEEDACCEPTANCERANGE)
  {
   ret = true;
   //TODO: reset encoders to 0
   SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
  }
  leftGeartoothPosition = SensorValue[leftEncoder];
  rightGeartoothPosition = SensorValue[rightEncoder];

  return ret;

}


//=====================================================
//turn is measured in degrees of turn from the center of the bot & 0 being straight forward, positive numbers turn it to the left.
bool driveTurn (int degree)
{
  //Calculations assume Bot radius=8", 1 wheel rotation = 12.56", 1 encoder rotation=90 clicks
  int target = .332 * (degree * 4);
  int temp2;
  motor[port2]= motor[port1]= motor[port9]=limitMotorPower (getPID(-target,SensorValue[leftEncoder],1,TARGET_KP,TARGET_KI));//+ 0*getPID(-SensorValue[rightEncoder],SensorValue[leftEncoder],3,0,1));
  motor[port3]= motor[port4]= motor[port10]= temp2 = limitMotorPower (getPID(target,SensorValue[rightEncoder],2,TARGET_KP,TARGET_KI));//+ 0*getPID(-SensorValue[leftEncoder],SensorValue[rightEncoder],4,0,1));

  bool ret = 0;
  if (withinRange(SensorValue[rightEncoder] ,target,ENCODERACCEPTANCERANGE) && withinRange(SensorValue[leftEncoder] , -target,ENCODERACCEPTANCERANGE) && SensorValue[rightEncoder] - rightGeartoothPosition < ENCODERACCEPTANCERANGE && SensorValue[leftEncoder] - leftGeartoothPosition < ENCODERACCEPTANCERANGE)
  {
    ret = true;

    SensorValue[rightEncoder]=SensorValue[leftEncoder]=0;
  }
  leftGeartoothPosition = SensorValue[leftEncoder];
  rightGeartoothPosition = SensorValue[rightEncoder];
  return ret;
}





/*void testAuto()
{
 while (driveStraight(100)==false)
 {

 }

 while(vexRT[btn8R]==0)
 {


 }

 while (driveStraight(-100)==false)
  {

  }

 while(vexRT[btn8R]==1)
 {


 }

  while (driveTurn(90)==false)
  {

  }

 while(vexRT[btn8R]==0)
 {


 }

 while (driveTurn (180)==false)
 {

 }

 testAuto();

}*/
